/*
 * Copyright (c) 2006-2024, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-06     RT-Thread    first version
 * ctrl shift + 字体变大
 * ctrl - 字体变小
 */
#include<stdlib.h>
#include<math.h>
#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#define DBG_TAG "main"
#include <rtdbg.h>
#include <stdint.h>
#include "hmc5883.h"
#include "sensor_intf_hmc5883.h"
#include "sensor.h"
#include <math.h>
#include "gps_rmc.h"
/********************************************************************/
//全局变量 懒得搞邮箱通信了
char signalCh=0;//串口接收到的字符（对电机的控制指令），在对411通信中直接判断
double LonRecv=0;//全局接收经度
double LatRecv=0;//全局接收纬度
double LonShip=0;//无人船经度
double LatShip=0;//无人船纬度
char lon_flag=0;//确认接收数据是经度
char lat_flag=0;//确认接收数据是纬度
char LonFlag=0;//确认接收到经度
char LatFlag=0;//确认接收到纬度
char GPSFlag=0;//要确保lon_flag和lat_flat都是1才是完整数据
char GoFlag = 0;//找到方向前进标志位
char FindFlag = 0;
float Magangle;//方位角
double   pi = 3.1415926535897932384626;
//TDS
uint16_t temp=0,TDS=0;
rt_device_t uart1_dev;
rt_thread_t th_u1;
struct serial_configure u1_configs = RT_SERIAL_CONFIG_9600;
/*********************************************************************/
/*信号量、线程间防止变量没有计算完就被读取*/
rt_sem_t sem1;
/*定时器*/
int tim_s=0;
char tim_start_flag=0;
/********************************************************************/
//hmc自检函数
void hmc5883l_selftest(rt_device_t devi,void *buffer,struct hmc5883_device *dev,struct hmc5883_3axes *mag,float *Xoffest,float *Yoffest,float *Kx,float *Ky){
        signalCh=3;
        uint8_t i=0 ;

        float GaX,GaY,GaXmax=0,GaXmin=0,GaYmax=0,GaYmin=0;

    rt_kprintf("start");
        while(i != 100)
    {
        rt_device_read(devi, 0, buffer, 1);//调用rt设备读取
        //hmc5883l_rawread(&GaX, &GaY);
        hmc5883_get_mag_raw(dev,mag);

        GaX=mag->x;
        GaY=mag->y;

        GaXmax = GaXmax < GaX? GaX:GaXmax;

        GaXmin = GaXmin > GaX? GaX:GaXmin;

        GaYmax = GaYmax < GaY? GaY:GaYmax;

        GaYmin = GaYmin > GaY? GaY:GaYmin;
        rt_kprintf("gax:%f gay:%f ",GaX,GaY);
        rt_kprintf("gaxmax:%f gaxmin:%f gaymax:%f gaymin:%f\n",GaXmax,GaXmin,GaYmax,GaYmin);
        rt_thread_delay(70);

        i++;

                rt_kprintf("...");
    }
        rt_kprintf("end");
    *Xoffest = (GaXmax+GaXmin)/2;
    *Yoffest = (GaYmax+GaYmin)/2;
    *Kx = 2/(GaXmax-GaXmin);
    *Ky = 2/(GaXmax-GaXmin);
    rt_kprintf("xoff:%f yoff:%f kx:%f ky:%f\n",*Xoffest,*Yoffest,*Kx,*Ky);
}

//hmc数值转换函数转换为角度

int16_t hmc5883l_read(rt_device_t devi,void *buffer,struct hmc5883_device *dev,struct hmc5883_3axes *mag,float Xoffest,float Yoffest,float Kx,float Ky){

        float rawGaX,rawGaY;

        int16_t Magangle;
        rt_device_read(devi, 0, buffer, 1);//调用rt设备读取
        hmc5883_get_mag_raw(dev,mag);
        rawGaX=mag->x;
        rawGaY=mag->y;

        float GaX = (rawGaX - Xoffest) * Kx;

        float GaY = (rawGaY - Yoffest) * Ky;

            if((GaX > 0)&&(GaY > 0)) Magangle = atan(GaY/GaX)*57;
            else if((GaX > 0)&&(GaY < 0)) Magangle = 360+atan(GaY/GaX)*57;
            else if((GaX == 0)&&(GaY > 0)) Magangle = 90;
            else if((GaX == 0)&&(GaY < 0)) Magangle = 270;
            else if(GaX < 0) Magangle = 180+atan(GaY/GaX)*57;

            return Magangle;

}

//hmc线程入口函数
struct hmc5883_device *mag;//hmc5883设备结构体
rt_thread_t hmc_ptr = NULL;//hmc线程指针
void mag_thread_entry1(void* parameter)
{
    rt_device_t dev = RT_NULL;//rtt设备
    struct rt_sensor_data sensor_data;//sensor传感器设备
    rt_size_t res;//返回值判断

    struct hmc5883_3axes mag1;//hmc5883的3元数据接收
    struct rt_sensor_config cfg;//注册sensor设备
    cfg.intf.dev_name = "i2c2";//sensor设备中的iic总线
    cfg.intf.user_data = (void *)HMC5883_ADDR;//hmc的iic地址
    rt_hw_hmc5883_init("hmc", &cfg);//注册hmc传感器设备
    mag=hmc5883_init("i2c2",HMC5883_ADDR);//hmc初始化返回hmc设备结构体

    dev = rt_device_find("mag_hmc");//查找设备
//    if (dev == RT_NULL)
//    {
//        rt_kprintf("Can't find device:%s\n", "mag_hmc");
//        return;
//    }
    rt_device_open(dev, RT_DEVICE_FLAG_RDWR);
//    if (rt_device_open(dev, RT_DEVICE_FLAG_RDWR) != RT_EOK)
//    {
//        rt_kprintf("open device failed!\n");
//        return;
//    }
    //自检部分代码
    float Xoffest,Yoffest,Kx,Ky;
    while (1)
    {

        rt_thread_delay(200);//避免线程死循环

        Magangle=hmc5883l_read(dev,&sensor_data,mag,&mag1,Xoffest,Yoffest,Kx,Ky);
//        rt_kprintf("\r\nentry mangle:%f\r\n",Magangle);
        if(signalCh==5)//收到校准指令
        {
            rt_kprintf("shudaol!!\r\n");
            hmc5883l_selftest(dev,&sensor_data,mag,&mag1,&Xoffest,&Yoffest,&Kx,&Ky);
            signalCh=0;
        }


    }

}


//hmc入口函数
void hmc_proc(void)
{
    hmc_ptr = rt_thread_create("hmc_demo", mag_thread_entry1, NULL, 1024, 5, 100);
//    if(hmc_ptr == RT_NULL)
//    {
//        LOG_E("rt_thread_create failed\n");
//    }
//    LOG_D("rt_thread_create SUCCESSED\n");
    rt_thread_startup(hmc_ptr);
}
/*******************************************************************************************/
#define SAMPLE_UART_NAME       "uart3"

/* 用于接收消息的信号量 */
static struct rt_semaphore rx_sem;
static rt_device_t serial;

/* 接收数据回调函数 */
static rt_err_t uart_input(rt_device_t dev, rt_size_t size)
{
    /* 串口接收到数据后产生中断，调用此回调函数，然后发送接收信号量 */
    rt_sem_release(&rx_sem);

    return RT_EOK;
}

/*static void send_thread_entry(void *parameter)//发送
{
    char ch;
    char buf1[10];
    int tset=1;
    while (1)
    {

        sprintf((char*)buf1, "{Long:%d}",  tset);
        rt_device_write(serial, 0, buf1, sizeof(buf1));
        rt_thread_mdelay(500);
    }
}*/

/***
***  接收到的控制信号格式为{"s":"a","d":"a"}
***                      01234567890123456
***                      {"lat":"34.621454018587755"}
***                      {"lon":"113.69276304871846"}
 */
static void serial_thread_entry(void *parameter)//接收
{
    char ch;
    char num=0;
    char signalFlag=0;
    double lon_recv_decimal=0;
    double lat_recv_decimal=0;
    char ntemp=0;
    long d=10;
    while (1)
    {
        /* 从串口读取一个字节的数据，没有读取到则等待接收信号量 */
        while (rt_device_read(serial, -1, &ch, 1) != 1)
        {
             //阻塞等待接收信号量，等到信号量后再次读取数据
            rt_sem_take(&rx_sem, RT_WAITING_FOREVER);
        }
        num++;
        rt_kprintf("%c",ch);

        if(ch=='}')//结束位标志
        {
            num=0;
            lon_flag=0;
            lat_flag=0;
            signalFlag=0;
            GPSFlag=0;
            d=10;
            lat_recv_decimal=0;
            lon_recv_decimal=0;
//            LatRecv=0;
//            LonRecv=0;
        }
//1
        if(num==12 && ch=='.')//根据小数点位置判断是lat还是lon，现在是lon
        {
            lon_flag=1;
            continue;
        }
        if(lon_flag==1)//开始处理小数部分
        {
            if(ch=='\"')//结束
            {
                LonRecv=113+lon_recv_decimal;
                rt_kprintf("\r\nLonRecv:%.8lf\n",LonRecv);
                LonFlag=1;
                continue;
            }
            ntemp=ch-'0';
            lon_recv_decimal+=((double)ntemp)/d;

            d*=10;
        }
//1


        if(num==11 && ch=='.')//根据小数点位置判断是lat还是lon，现在是lat
                {
                    lat_flag=1;
//                    rt_kprintf("lat_flag:%d\n",lat_flag);
                    continue;
                }
                if(lat_flag==1)//开始处理小数部分
                {
                    if(ch=='\"')//结束
                    {
                        LatRecv=34+lat_recv_decimal;
                        rt_kprintf("\r\nLatRecv:%.8lf\n",LatRecv);
                        LatFlag=1;
                        continue;
                    }
                    ntemp=ch-'0';
                    lat_recv_decimal+=((double)ntemp)/d;
//                    rt_kprintf(" %d %lf\r\n",d,lat_recv_decimal);
                    d*=10;
                }

        if(num==7 && ch == 'a')//说明是信号
        {
            signalFlag=1;

//            rt_kprintf("\r\nsignalflag:%d\r\n",signalFlag);
        }
        if(signalFlag==1)
        {
            if(num==15)
            {
                switch (ch) {
                    case '1':
                        signalCh = 1;
                        break;
                    case '2':
                        signalCh = 2;
                        break;
                    case '0':
                        signalCh = 0;
                        break;
                    case '3':
                        signalCh = 3;
                        break;
                    case '4':
                        signalCh = 4;
                        break;
                    case 'j':
                        signalCh = 5;
                        break;
                }
            }
        }



    }
}

static int uart_sample(int argc, char *argv[])
{
    rt_err_t ret = RT_EOK;
    char uart_name[RT_NAME_MAX];
    char str[] = "hello RT-Thread!\r\n";

    if (argc == 2)
    {
        rt_strncpy(uart_name, argv[1], RT_NAME_MAX);
    }
    else
    {
        rt_strncpy(uart_name, SAMPLE_UART_NAME, RT_NAME_MAX);
    }

    /* 查找系统中的串口设备 */
    serial = rt_device_find(uart_name);
    if (!serial)
    {
        rt_kprintf("find %s failed!\n", uart_name);
        return RT_ERROR;
    }

    /* 初始化信号量 */
    rt_sem_init(&rx_sem, "rx_sem", 0, RT_IPC_FLAG_FIFO);
    /* 以中断接收及轮询发送模式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_FLAG_INT_RX);
    /* 设置接收回调函数 */
    rt_device_set_rx_indicate(serial, uart_input);
    /* 发送字符串 */

    rt_device_write(serial, 0, str, (sizeof(str) - 1));

    /* 创建 serial 线程 */
    rt_thread_t thread = rt_thread_create("serial", serial_thread_entry, RT_NULL, 1024, 25, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    return ret;
}

/* 导出到 msh 命令列表中*/
INIT_APP_EXPORT(uart_sample);
/*******************************************************************************/
/***
 *** 负责发送无人船gps坐标
 ***
 ***
 *** ***/
gps_info_t gps;
//extern gps_info_t info;
void test1_entry(void)
{

    char buf1[60];

    while(1)
    {
        rt_thread_mdelay(1000);
        sprintf((char*)buf1, "{Long:%s,lat:%s,TDS:%d,TEM:%d}",  info->coord.location.latitude.string, info->coord.location.longitude.string,TDS,temp);
        LatShip = strtod(info->coord.location.latitude.string,NULL);
        LonShip= strtod(info->coord.location.longitude.string,NULL);
        rt_device_write(serial, 0, buf1, strlen(buf1));

        rt_memset(info, 0, sizeof(struct gps_info));


    }
}

void test1_start_proc(void)
{
    rt_thread_t test1;
    test1 = rt_thread_create("test1", test1_entry, RT_NULL, 1024, 25, 10);
    /* 创建成功则启动线程 */
    if (test1 != RT_NULL)
    {
        rt_thread_startup(test1);
    }
    else
    {
        rt_kprintf("test program wrong!\n");
    }
}
/* 导出到 msh 命令列表中*/
INIT_APP_EXPORT(test1_start_proc);
/**************************************************************************/
/***
***与411通信部分，仅需要三个IO口，需要五个数据 000 001 010 011 100
***  PC9 PC8 PC6 PIN_HIGH代表1 PIN_LOW代表0
***  41  40  38
***  0   0   0  停止0
***  0   0   1  前进1
***  0   1   0  左转2
***  0   1   1  右转3
***  1   0   0  后退4
***  接收到的控制信号格式为{"s":"1","d":"s"}
***                      01234567890123456
***/
void GOForward(void)
{
    rt_pin_write(41, PIN_LOW);
    rt_pin_write(40, PIN_LOW);
    rt_pin_write(38, PIN_HIGH);
}
void GoLeft(void)
{
    rt_pin_write(41, PIN_LOW);
    rt_pin_write(40, PIN_HIGH);
    rt_pin_write(38, PIN_LOW);
}
void GoRight(void)
{
    rt_pin_write(41, PIN_LOW);
    rt_pin_write(40, PIN_HIGH);
    rt_pin_write(38, PIN_HIGH);
}
void GoBack(void)
{
    rt_pin_write(41, PIN_HIGH);
    rt_pin_write(40, PIN_LOW);
    rt_pin_write(38, PIN_LOW);
}
void StopGo(void)
{
    rt_pin_write(41, PIN_LOW);
    rt_pin_write(40, PIN_LOW);
    rt_pin_write(38, PIN_LOW);
}

void signalTo411(void)
{
    rt_pin_mode(41, PIN_MODE_OUTPUT);//PC9
    rt_pin_mode(40, PIN_MODE_OUTPUT);//PC8
    rt_pin_mode(38, PIN_MODE_OUTPUT);//PC6


    while(1)
    {
        rt_thread_mdelay(50);
        if(signalCh==1)//前进1
        {
            GOForward();

        }
        if(signalCh==2)//左转2
        {
            GoLeft();
        }
        if(signalCh==3)//右转3
        {
            GoRight();
        }
        if(signalCh==4)//后退4
        {
            GoBack();
        }
        if(signalCh==0)//停止0
        {
            StopGo();
            GoFlag=0;
            FindFlag=0;
        }
    }


}

void signalTo411_proc(void)
{
    rt_thread_t signal411;
    signal411 = rt_thread_create("signal411", signalTo411, RT_NULL, 1024, 25, 10);
    /* 创建成功则启动线程 */
    if (signal411 != RT_NULL)
    {
        rt_thread_startup(signal411);
    }
//    else
//    {
//        rt_kprintf("signal411 program wrong!\n");
//    }
}
INIT_APP_EXPORT(signalTo411_proc);
/***********************************************************/
/***自动巡航处理部分
 *方位角的计算
 *巡航坐标的范围区域计算
 *接收到的巡航点坐标 LonRecv全局接收经度LatRecv全局接收纬度
 *需要在同时接收到经纬度后才让GpsFlag为1，说明数据完整
 *无人船的坐标
 * ***/
/************新增******************/
void RectifyAngle(double ShipAngle,double TargetAngle)
{
    if(TargetAngle<180 && TargetAngle>0)//目标角大于0小于180
    {
        if(TargetAngle>ShipAngle || ShipAngle>TargetAngle+180)
        {
            //向左转对应信号110
            rt_pin_write(41, PIN_HIGH);
            rt_pin_write(40, PIN_HIGH);
            rt_pin_write(38, PIN_LOW);
        }
        if(TargetAngle<ShipAngle && ShipAngle<TargetAngle+180)
        {
            //向右转对应信号111
            rt_pin_write(41, PIN_HIGH);
            rt_pin_write(40, PIN_HIGH);
            rt_pin_write(38, PIN_HIGH);
        }
    }
    if(TargetAngle<360 && TargetAngle>180)//目标角大于180小于360
    {
        if(TargetAngle<ShipAngle || ShipAngle<TargetAngle-180)
        {
            //向右转对应信号111
            rt_pin_write(41, PIN_HIGH);
            rt_pin_write(40, PIN_HIGH);
            rt_pin_write(38, PIN_HIGH);
        }
        if(TargetAngle>ShipAngle && ShipAngle>TargetAngle-180)
        {
            //向左转对应信号110
            rt_pin_write(41, PIN_HIGH);
            rt_pin_write(40, PIN_HIGH);
            rt_pin_write(38, PIN_LOW);
        }
    }
}
/************新增******************/

/*
 * lon1\lat1应该传入船的坐标
 * lon2\aat2应该传入巡航点的坐标
 * */




double get_angle(double lon1,double lat1,double lon2,double lat2)
{
    double deg2rad = pi/180;
    double dlat =(lat2 - lat1) * deg2rad;
    double dlon = (lon2 - lon1) * deg2rad;
    double y = sin(dlon)*cos(lat2*deg2rad);
    double x = cos(lat1*deg2rad)*sin(lat2*deg2rad)-sin(lat1*deg2rad)*cos(lat2*deg2rad)*cos(dlon);
    double angle = atan2(y,x)*180/pi;
    return angle;
}

double CutOut5(double num)
{
    num=(long long)(100000*num);
    num=num/100000;
    return num;
}

void AutoDrive_entry(void)
{
    double compute_angle;



    while(1)
    {
        rt_sem_take(sem1, RT_WAITING_FOREVER);
        //不断获取的信息 无人船所在的经纬度 方位角
//        rt_kprintf("findflag:%d\n",FindFlag);
        if(LonFlag==1 && LatFlag==1)//判断接收经纬度信息是否完整
        {
            GPSFlag=1;//完整
        }
        if(GPSFlag==1)//收到的是完整数据、可以计算方位角了
        {
            //计算方位角
//            LonShip5=CutOut5(LonShip);
//            LatShip5=CutOut5(LatShip);
//            LonRecv5=CutOut5(LonRecv);
//            LatRecv5=CutOut5(LatRecv);
            compute_angle = get_angle(LatShip,LonShip , LonRecv, LatRecv);//获得无人船所面向的方向的方位角
            if(compute_angle<0)
            {
                compute_angle=compute_angle+360;
            }
//            rt_kprintf("LonShip:%lf, LatShip:%lf\r\n",LonShip,LatShip);
//            rt_kprintf("LonRecv:%lf, LatRecv:%lf\r\n", LonRecv, LatRecv);
//            rt_kprintf("compute_angle:%lf\r\n",compute_angle);
            GPSFlag=0;
            LonFlag=0;
            LatFlag=0;

            FindFlag=1;
        }
        if(FindFlag==1)
        {
            signalCh=2;
//            rt_kprintf("\r\nMship:%f\r\n",Magangle);
//            rt_kprintf("\r\ncomangle:%f\r\n",compute_angle);
            if(Magangle>compute_angle-15 && Magangle<compute_angle+15)
                //判断方位角Magangle是否在compute_angle的+-5的范围
            {
                signalCh=3;//反转一下
                rt_thread_mdelay(500);//防止惯性
                signalCh=0;
                FindFlag=0;
                GoFlag = 1;//可以直行了
                tim_start_flag=1;

            }
        }
        if(GoFlag==1)//开始直行
        {
            rt_kprintf("g0flag:%d\n",GoFlag);
            //直行GoFlag为1时这个括号里不断执行
            /************新增******************/
            RectifyAngle((double)Magangle,compute_angle);
            rt_thread_mdelay(100);
            /************新增******************/
            signalCh=1;
//            rt_kprintf("LonShip:%lf, LatShip:%lf\r\n",LonShip,LatShip);
//            rt_kprintf("LonRecv:%lf, LatRecv:%lf\r\n", LonRecv, LatRecv);
//            rt_kprintf("LatShip-LonRecv:%lf\r\n",LatShip-LonRecv);
//            rt_kprintf("LonShip-LatRecv:%lf\r\n",LonShip-LatRecv);
            if(fabs(LatShip-LonRecv)<=0.00003 && fabs(LonShip-LatRecv)<=0.00003 )//判断是否到了巡航点范围
            {
                signalCh=4;//减速
                rt_thread_mdelay(500);
                signalCh=0;
                GoFlag = 0;
                LatRecv=0;
                LonRecv=0;
                FindFlag=0;
             }
//            if(fabs(LonShip-LonRecv)<=0.00003 && fabs(LatShip-LatRecv)<=0.00003 )//判断是否到了巡航点范围
//            {
//                signalCh=4;//减速
//                rt_thread_mdelay(500);
//                signalCh=0;
//                GoFlag = 0;
//            }
            //保护机制，万一一直向前走了，超时,整个定时器
            if(tim_start_flag==0)
            {
                signalCh=0;
                GoFlag = 0;
            }
        }
        rt_sem_release(sem1);
        rt_thread_mdelay(500);

    }
}

void AutoDrive_start_proc(void)
{
    rt_thread_t test1;
    test1 = rt_thread_create("AUtoDrive", AutoDrive_entry, RT_NULL, 1024, 20, 10);
    /* 创建成功则启动线程 */
    if (test1 != RT_NULL)
    {
        rt_thread_startup(test1);
    }
//    else
//    {
//        rt_kprintf("AutoDrive program wrong!\n");
//    }
}
/* 导出到 msh 命令列表中*/
INIT_APP_EXPORT(AutoDrive_start_proc);
/******************************************************/
/*软件定时器*/
void SoftTim_callback(void)
{
    tim_s++;
    if(tim_start_flag==1)
        tim_s=0;
    if(tim_s == 40)
    {
        tim_s=0;
        tim_start_flag=0;
    }

}
/**************************************************/


void temp_tds_read(uint8_t *r_table)
{
    if((r_table[0]+r_table[1]+r_table[2]+r_table[3]+r_table[4])==(r_table[5]*256+r_table[6]))   //验证校验和
            {
             temp=r_table[1]*256+r_table[2];      //计算温度 单位是0.1度 若temp=235  则温度是23.5度
             TDS =r_table[3]*256+r_table[4];    //计算TDS   单位是1ppm

                //以上计算的出来的数据 根据自己需要进行处理 （显示在液晶上或者其他地方）

        //  如果PA9不接TDS模块  可以连接外部的串口助手 用于查看打印输出的温度+TDS数值  一定！！把下面的一行取消注释就可以的！！！！
//             rt_kprintf("temp=%d.%d ,TDS=%d\r\n",temp/10,temp%10,TDS);
            }
}


void read_entry(void *parameter)
{
    char buffer[7];
    while(1)
    {
        rt_device_read(uart1_dev, 0, buffer, sizeof(buffer));
        temp_tds_read(buffer);

        rt_thread_delay(1000);
    }
}



int main(void)
{
    rt_timer_t tm;

    sem1 = rt_sem_create("sem1", 1, RT_IPC_FLAG_FIFO);
    tm = rt_timer_create("time_alarm", SoftTim_callback, NULL, 1000, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
    rt_timer_start(tm);
    rt_err_t ret = 0;
    uart1_dev = rt_device_find("uart2");
    ret = rt_device_open(uart1_dev, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX);//打开读写，打开中断接收
    rt_device_control(uart1_dev, RT_DEVICE_CTRL_CONFIG, (void*)&u1_configs);//配置信息
    th_u1 = rt_thread_create("u1_recv", read_entry, NULL, 1024, 10, 5);//创建线程
    rt_thread_startup(th_u1);

    hmc_proc();




    return RT_EOK;
}
